
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       srv_channel.h
  * @author     baiyang
  * @date       2021-12-29
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdint.h>
#include <stdbool.h>

#include <common/gp_config.h>
#include <srv_channel/hal/srv_hal_mgr.h>
/*-----------------------------------macro------------------------------------*/
#ifndef NUM_SERVO_CHANNELS
#if defined(HAL_BUILD_AP_PERIPH) && defined(HAL_PWM_COUNT)
    #define NUM_SERVO_CHANNELS HAL_PWM_COUNT
#elif defined(HAL_BUILD_AP_PERIPH)
    #define NUM_SERVO_CHANNELS 0
#else
    #define NUM_SERVO_CHANNELS 16
#endif
#endif
/*----------------------------------typedef-----------------------------------*/
typedef enum
{
    k_GPIO                  = -1,           ///< used as GPIO pin (input or output)
    k_none                  = 0,            ///< disabled
    k_manual                = 1,            ///< manual, just pass-thru the RC in signal
    k_flap                  = 2,            ///< flap
    k_flap_auto             = 3,            ///< flap automated
    k_aileron               = 4,            ///< aileron
    k_unused1               = 5,            ///< unused function
    k_mount_pan             = 6,            ///< mount yaw (pan)
    k_mount_tilt            = 7,            ///< mount pitch (tilt)
    k_mount_roll            = 8,            ///< mount roll
    k_mount_open            = 9,            ///< mount open (deploy) / close (retract)
    k_cam_trigger           = 10,           ///< camera trigger
    k_egg_drop              = 11,           ///< egg drop, deprecated
    k_mount2_pan            = 12,           ///< mount2 yaw (pan)
    k_mount2_tilt           = 13,           ///< mount2 pitch (tilt)
    k_mount2_roll           = 14,           ///< mount2 roll
    k_mount2_open           = 15,           ///< mount2 open (deploy) / close (retract)
    k_dspoilerLeft1         = 16,           ///< differential spoiler 1 (left wing)
    k_dspoilerRight1        = 17,           ///< differential spoiler 1 (right wing)
    k_aileron_with_input    = 18,            ///< aileron, with rc input, deprecated
    k_elevator              = 19,            ///< elevator
    k_elevator_with_input   = 20,            ///< elevator, with rc input, deprecated
    k_rudder                = 21,            ///< secondary rudder channel
    k_sprayer_pump          = 22,            ///< crop sprayer pump channel
    k_sprayer_spinner       = 23,            ///< crop sprayer spinner channel
    k_flaperon_left         = 24,            ///< flaperon, left wing
    k_flaperon_right        = 25,            ///< flaperon, right wing
    k_steering              = 26,            ///< ground steering, used to separate from rudder
    k_parachute_release     = 27,            ///< parachute release
    k_gripper               = 28,            ///< gripper
    k_landing_gear_control  = 29,            ///< landing gear controller
    k_engine_run_enable     = 30,            ///< engine kill switch, used for gas airplanes and helicopters
    k_heli_rsc              = 31,            ///< helicopter RSC output
    k_heli_tail_rsc         = 32,            ///< helicopter tail RSC output
    k_motor1                = 33,            ///< these allow remapping of copter motors
    k_motor2                = 34,
    k_motor3                = 35,
    k_motor4                = 36,
    k_motor5                = 37,
    k_motor6                = 38,
    k_motor7                = 39,
    k_motor8                = 40,
    k_motor_tilt            = 41,            ///< tiltrotor motor tilt control
    k_generator_control     = 42,            ///< state control for generator
    k_tiltMotorRear         = 45,            ///<vectored thrust, rear tilt
    k_tiltMotorRearLeft     = 46,            ///<vectored thrust, rear left tilt
    k_tiltMotorRearRight    = 47,            ///<vectored thrust, rear right tilt
    k_rcin1                 = 51,            ///< these are for pass-thru from arbitrary rc inputs
    k_rcin2                 = 52,
    k_rcin3                 = 53,
    k_rcin4                 = 54,
    k_rcin5                 = 55,
    k_rcin6                 = 56,
    k_rcin7                 = 57,
    k_rcin8                 = 58,
    k_rcin9                 = 59,
    k_rcin10                = 60,
    k_rcin11                = 61,
    k_rcin12                = 62,
    k_rcin13                = 63,
    k_rcin14                = 64,
    k_rcin15                = 65,
    k_rcin16                = 66,
    k_ignition              = 67,
    k_choke                 = 68,           /// not used
    k_starter               = 69,
    k_throttle              = 70,
    k_tracker_yaw           = 71,            ///< antennatracker yaw
    k_tracker_pitch         = 72,            ///< antennatracker pitch
    k_throttleLeft          = 73,
    k_throttleRight         = 74,
    k_tiltMotorLeft         = 75,            ///< vectored thrust, left tilt
    k_tiltMotorRight        = 76,            ///< vectored thrust, right tilt
    k_elevon_left           = 77,
    k_elevon_right          = 78,
    k_vtail_left            = 79,
    k_vtail_right           = 80,
    k_boost_throttle        = 81,            ///< vertical booster throttle
    k_motor9                = 82,
    k_motor10               = 83,
    k_motor11               = 84,
    k_motor12               = 85,
    k_dspoilerLeft2         = 86,           ///< differential spoiler 2 (left wing)
    k_dspoilerRight2        = 87,           ///< differential spoiler 2 (right wing)
    k_winch                 = 88,
    k_mainsail_sheet        = 89,           ///< Main Sail control via sheet
    k_cam_iso               = 90,
    k_cam_aperture          = 91,
    k_cam_focus             = 92,
    k_cam_shutter_speed     = 93,
    k_scripting1            = 94,           ///< Scripting related outputs
    k_scripting2            = 95,
    k_scripting3            = 96,
    k_scripting4            = 97,
    k_scripting5            = 98,
    k_scripting6            = 99,
    k_scripting7            = 100,
    k_scripting8            = 101,
    k_scripting9            = 102,
    k_scripting10           = 103,
    k_scripting11           = 104,
    k_scripting12           = 105,
    k_scripting13           = 106,
    k_scripting14           = 107,
    k_scripting15           = 108,
    k_scripting16           = 109,
    k_airbrake              = 110,
    k_LED_neopixel1         = 120,
    k_LED_neopixel2         = 121,
    k_LED_neopixel3         = 122,
    k_LED_neopixel4         = 123,
    k_roll_out              = 124,
    k_pitch_out             = 125,
    k_thrust_out            = 126,
    k_yaw_out               = 127,
    k_wingsail_elevator     = 128,
    k_ProfiLED_1            = 129,
    k_ProfiLED_2            = 130,
    k_ProfiLED_3            = 131,
    k_ProfiLED_Clock        = 132,
    k_winch_clutch          = 133,
    k_min                   = 134,  // always outputs SERVOn_MIN
    k_trim                  = 135,  // always outputs SERVOn_TRIM
    k_max                   = 136,  // always outputs SERVOn_MAX
    k_mast_rotation         = 137,
    k_nr_aux_servo_functions        ///< This must be the last enum value (only add new values _before_ this one)
} Aux_servo_function_t;

// used to get min/max/trim limit value based on reverse
enum SRVLimit {
    SRV_TRIM,
    SRV_MIN,
    SRV_MAX,
    SRV_ZERO_PWM
};

// a bitmask type wide enough for NUM_SERVO_CHANNELS
typedef uint16_t servo_mask_t;

typedef struct {
    Param_int16 servo_min;
    Param_int16 servo_max;
    Param_int16 servo_trim;

    // reversal, following convention that 1 means reversed, 0 means normal
    Param_int8 reversed;
    Aux_servo_function_t function;

    // a pending output value as PWM
    uint16_t output_pwm;

    // true for angle output type
    bool type_angle:1;

    // set_range() or set_angle() has been called
    bool type_setup:1;

    // the hal channel number
    uint8_t ch_num;

    // high point of angle or range output
    uint16_t high_out;

    // previous radio_in during pass-thru
    int16_t previous_radio_in;

    // specify that small rcinput changes should be ignored during passthrough
    // used by DO_SET_SERVO commands
    bool ign_small_rcin_changes;

    // if true we should ignore all imputs on this channel
    bool override_active;

} srv_channel;

typedef struct {
    Param_int8 auto_trim;
    Param_int16 default_rate;
    Param_int8 dshot_rate;
    Param_int8 dshot_esc_type;
    Param_int32 gpio_mask;

    bool emergency_stop;

    bool disabled_passthrough;
    uint16_t disabled_mask;

    // mask of outputs which use a digital output protocol, not
    // PWM (eg. DShot)
    uint16_t digital_mask;

    // mask of outputs which are digitally reversible (eg. DShot-3D)
    uint16_t reversible_mask;

    srv_channel obj_channels[NUM_SERVO_CHANNELS];

    // override loop counter
    uint16_t override_counter[NUM_SERVO_CHANNELS];

    struct srv_function {
        // mask of what channels this applies to
        servo_mask_t channel_mask;

        // scaled output for this function
        float output_scaled;
    } functions[k_nr_aux_servo_functions];

    servo_mask_t trimmed_mask;

    uint32_t function_mask[(k_nr_aux_servo_functions+31)/32];

    bool initialised;

    // this static arrangement is to avoid having static objects in AP_Param tables
    srv_channel *channels;
} srv_channels;

/*----------------------------------variable----------------------------------*/
extern srv_channels srvs;
extern servo_mask_t srv_have_pwm_mask;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/// srv_channel
void srv_channel_ctor(srv_channel* channel, uint8_t ch_num);

void srv_channel_calc_pwm(srv_channel* channel, float output_scaled);

// return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous
bool srv_channel_should_e_stop(Aux_servo_function_t function);

void srv_channel_set_output_pwm(srv_channel* channel, uint16_t pwm, bool force);

// set normalised output from -1 to 1, assuming 0 at mid point of servo_min/servo_max
void srv_channel_set_output_norm(srv_channel* channel, float value);

// set angular range of scaled output
void srv_channel_set_angle(srv_channel* channel, int16_t angle);

// set range of scaled output
void srv_channel_set_range(srv_channel* channel, uint16_t high);

/*
  get normalised output from -1 to 1, assuming 0 at mid point of servo_min/servo_max
 */
float srv_channel_get_output_norm(srv_channel* channel);

uint16_t srv_channel_get_limit_pwm(srv_channel* channel, enum SRVLimit limit);

// return true if function is for a multicopter motor
static inline bool srv_channel_is_motor(Aux_servo_function_t function) {
    return ((function >= k_motor1 && function <= k_motor8) ||
            (function >= k_motor9 && function <= k_motor12));
}

// return true if function is for a control surface
bool srv_channel_is_control_surface(Aux_servo_function_t function);

// return the motor number of a channel, or -1 if not a motor
// return 0 for first motor
int8_t srv_channel_get_motor_num(srv_channel* channel);

/// map a function to a servo channel and output it
void srv_channel_output_ch(srv_channel* channel);

/*
   setup a channels aux servo function
*/
void srv_channel_aux_servo_function_setup(srv_channel* channel);

servo_mask_t srv_channel_get_have_pwm_mask();
void srv_channel_have_pwm_mask(uint8_t chan);

static inline void srv_channel_set_override(srv_channel* channel, bool b) {channel->override_active = b;}

// check if a function is valid for indexing into functions
static inline bool srv_channel_valid_function2(Aux_servo_function_t fn) {
    return fn >= 0 && fn < k_nr_aux_servo_functions;
}

static inline bool srv_channel_valid_function(srv_channel* channel) {
    return srv_channel_valid_function2(channel->function);
}

// set MIN/MAX parameters
static inline void srv_channel_set_output_min(srv_channel* channel, uint16_t pwm) {
    channel->servo_min = pwm;
}
static inline void srv_channel_set_output_max(srv_channel* channel, uint16_t pwm) {
    channel->servo_max = pwm;
}

// return the function of a channel
static inline Aux_servo_function_t srv_channel_get_function(const srv_channel* channel) {
    return (Aux_servo_function_t)channel->function;
}

// get MIN/MAX/TRIM parameters
static inline uint16_t srv_channel_get_output_min(const srv_channel * channel) {
    return channel->servo_min;
}
static inline uint16_t srv_channel_get_output_max(const srv_channel * channel) {
    return channel->servo_max;
}
static inline uint16_t srv_channel_get_trim(const srv_channel * channel) {
    return channel->servo_trim;
}

///
/// srv_channel
///
void srv_channels_ctor();

// get singleton instance
srv_channels *srv_channels_get_singleton();

// SRV_Channels initialization
void srv_channels_init(void);

/*
  save adjusted trims
 */
void srv_channels_save_trim(void);

void srv_channels_setup_failsafe_trim_all_non_motors(void);

/*
  run calc_pwm for all channels
 */
void srv_channels_calc_pwm(void);

/*
  return the current function for a channel
*/
Aux_servo_function_t srv_channels_channel_function(uint8_t channel);

/*
  return true if a channel should be available as a GPIO
 */
bool srv_channels_is_GPIO(uint8_t channel);

/*
  wrapper around hal.rcout->cork()
 */
static inline void srv_channels_cork() {
    srv_hal_cork();
}

/*
  wrapper around hal.rcout->push()
 */
static inline void srv_channels_push() {
    srv_hal_push();
}

/*
  call output_ch() on all channels
 */
void srv_channels_output_ch_all(void);

/*
  return the current function for a channel
*/
Aux_servo_function_t srv_channels_channel_function(uint8_t channel);

/// setup the output range types of all functions
void srv_channels_update_aux_servo_function(void);

/// Should be called after the the servo functions have been initialized
/// called at 1Hz
void srv_channels_enable_aux_servos();

void srv_channels_set_digital_outputs(uint16_t dig_mask, uint16_t rev_mask);

/// enable output channels using a channel mask
void srv_channels_enable_by_mask(uint16_t mask);

/*
  set radio_out for all channels matching the given function type
 */
void srv_channels_set_output_pwm(Aux_servo_function_t function, uint16_t value);

/*
  return true if a particular function is assigned to at least one RC channel
 */
bool srv_channels_function_assigned(Aux_servo_function_t function);

/*
  set radio_out for all channels matching the given function type
  trim the output assuming a 1500 center on the given value
  reverses pwm output based on channel reversed property
 */
void srv_channels_set_output_pwm_trimmed(Aux_servo_function_t function, int16_t value);

/*
  set and save the trim value to current output for all channels matching
  the given function type
 */
void srv_channels_set_trim_to_servo_out_for(Aux_servo_function_t function);

/*
  setup failsafe value for an auxiliary function type to a Limit
 */
void srv_channels_set_failsafe_pwm(Aux_servo_function_t function, uint16_t pwm);

/*
  setup failsafe value for an auxiliary function type to a Limit
 */
void srv_channels_set_failsafe_limit(Aux_servo_function_t function, enum SRVLimit limit);

/*
  set radio output value for an auxiliary function type to a Limit
 */
void srv_channels_set_output_limit(Aux_servo_function_t function, enum SRVLimit limit);

/*
  set the default channel an auxiliary output function should be on
 */
bool srv_channels_set_aux_channel_default(Aux_servo_function_t function, uint8_t channel);

// find first channel that a function is assigned to
bool srv_channels_find_channel(Aux_servo_function_t function, uint8_t *chan);

/*
  get a pointer to first auxillary channel for a channel function
*/
srv_channel *srv_channels_get_channel_for(Aux_servo_function_t function, int8_t default_chan);

void srv_channels_set_output_scaled(Aux_servo_function_t function, float value);
float srv_channels_get_output_scaled(Aux_servo_function_t function);

/*
  get mask of output channels for a function
 */
uint16_t srv_channels_get_output_channel_mask(Aux_servo_function_t function);

// set the trim for a function channel to given pwm
void srv_channels_set_trim_to_pwm_for(Aux_servo_function_t function, int16_t pwm);

// set the trim for a function channel to min output of the channel honnoring reverse unless ignore_reversed is true
void srv_channels_set_trim_to_min_for(Aux_servo_function_t function, bool ignore_reversed);

// get pwm output for the first channel of the given function type.
bool srv_channels_get_output_pwm(Aux_servo_function_t function, uint16_t *value);

// call set_angle() on matching channels
void srv_channels_set_angle(Aux_servo_function_t function, uint16_t angle);

// call set_range() on matching channels
void srv_channels_set_range(Aux_servo_function_t function, uint16_t range);

// set MIN parameter for a function
void srv_channels_set_output_min_max(Aux_servo_function_t function, uint16_t min_pwm, uint16_t max_pwm);

// set RC output frequency on a function output
void srv_channels_set_rc_frequency(Aux_servo_function_t function, uint16_t frequency_hz);

// given a zero-based motor channel, return the k_motor function for that channel
static inline Aux_servo_function_t srv_channels_get_motor_function(uint8_t channel) {
    if (channel < 8) {
        return (Aux_servo_function_t)(k_motor1+channel);
    }
    return (Aux_servo_function_t)((k_motor9+(channel-8)));
}

// return true if all of the outputs in mask are digital
bool srv_channels_have_digital_outputs(uint16_t mask);

// Set E - stop
void srv_channels_set_emergency_stop(bool state);

// get E - stop
bool srv_channels_get_emergency_stop();

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



